{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 参考论文给出Nao的正逆运动学分析"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing\n",
    "init_printing(use_latex='mathjax')\n",
    "from sympy import Matrix\n",
    "import sympy\n",
    "from robot import *\n",
    "import almath\n",
    "connect_robot = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# LArm 正运动学分析\n",
    "\n",
    "## 参考matlab程序"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def DH(a, alpha, d, theta):\n",
    "    Taf = [[cos(theta),            -sin(theta),            0,              a],\n",
    "    [sin(theta)*cos(alpha), cos(theta)*cos(alpha),  -sin(alpha),    -sin(alpha)*d],\n",
    "    [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),     cos(alpha)*d],\n",
    "    [0,                     0,                      0,              1]]\n",
    "    return Matrix(Taf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Nao kinematics for the left arm\n",
    "shoulderOffsetY = 98\n",
    "elbowOffsetY = 15\n",
    "upperArmLength = 105\n",
    "shoulderOffsetZ = 100\n",
    "HandOffsetX = 57.75\n",
    "HandOffsetZ = 12.31\n",
    "LowerArmLength = 55.95\n",
    "HipOffsetZ = 85\n",
    "HipOffsetY = 50\n",
    "ThighLength = 100\n",
    "TibiaLength = 102.90\n",
    "FootHeight = 45.11\n",
    "NeckOffsetZ = 126.5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def RotXYZMatrix(xAngle,yAngle,zAngle):\n",
    "    Rx = rotx(xAngle)\n",
    "    Ry = roty(yAngle)\n",
    "    Rz = rotz(zAngle)\n",
    "    R = Rx*Ry*Rz\n",
    "    return r2t(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def fLeftHandH25(theta1,theta2,theta3,theta4,theta5):\n",
    "    global shoulderOffsetY\n",
    "    global shoulderOffsetZ\n",
    "    global elbowOffsetY\n",
    "    global LowerArmLength\n",
    "    global HandOffsetX\n",
    "    global upperArmLength\n",
    "    global HandOffsetZ\n",
    "    \n",
    "    base = Matrix.eye(4)\n",
    "    base[1,3] = shoulderOffsetY\n",
    "    base[2,3] = shoulderOffsetZ\n",
    "    \n",
    "    T1 = DH(0,-pi/2,0,theta1)\n",
    "    T2 = DH(0,pi/2,0,theta2+pi/2)\n",
    "    T3 = DH(elbowOffsetY,pi/2,upperArmLength,theta3)\n",
    "    T4 = DH(0,-pi/2,0,theta4)\n",
    "    T5 = DH(0,pi/2,0,theta5)\n",
    "    Tend1 = Matrix.eye(4)\n",
    "    Tend1[0,3] = LowerArmLength + HandOffsetX\n",
    "    Tend1[2,3] = -HandOffsetZ\n",
    "    R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "    Tend = R*Tend1\n",
    "    Tendend = base*T1*T2*T3*T4*T5*Tend\n",
    "    \n",
    "    rotZ = atan2(Tendend[1,0],Tendend[0,0])\n",
    "    rotY = atan2(-Tendend[2,0], sqrt(float(Tendend[2,1]**2 + Tendend[2,2]**2)))\n",
    "    rotX = atan2(Tendend[2,1],Tendend[2,2])\n",
    "    left = Matrix.zeros(6,1)\n",
    "    left[0:3,0] = Tendend[0:3,3]\n",
    "    left[3:6,0] = Matrix([rotX,rotY,rotZ])\n",
    "    return Tendend,left"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( \\left[\\begin{matrix}1.0 & 0 & 0 & 218.7\\\\0 & 1.0 & 0 & 113.0\\\\0 & 0 & 1.0 & 87.69\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right], \\quad \\left[\\begin{matrix}218.7\\\\113.0\\\\87.69\\\\0.0\\\\0.0\\\\0.0\\end{matrix}\\right]\\right )$$"
      ],
      "text/plain": [
       "⎛⎡1.0   0    0   218.7⎤, ⎡218.7⎤⎞\n",
       "⎜⎢                    ⎥  ⎢     ⎥⎟\n",
       "⎜⎢ 0   1.0   0   113.0⎥  ⎢113.0⎥⎟\n",
       "⎜⎢                    ⎥  ⎢     ⎥⎟\n",
       "⎜⎢ 0    0   1.0  87.69⎥  ⎢87.69⎥⎟\n",
       "⎜⎢                    ⎥  ⎢     ⎥⎟\n",
       "⎜⎣ 0    0    0    1.0 ⎦  ⎢ 0.0 ⎥⎟\n",
       "⎜                        ⎢     ⎥⎟\n",
       "⎜                        ⎢ 0.0 ⎥⎟\n",
       "⎜                        ⎢     ⎥⎟\n",
       "⎝                        ⎣ 0.0 ⎦⎠"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "fLeftHandH25(0,0,0,0,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( \\left[\\begin{matrix}0.954390573728167 & -0.175813402219865 & 0.241305367488368 & 204.97147134049\\\\0.291150177124445 & 0.72701976605264 & -0.621831033424007 & 174.319793561599\\\\-0.0661075422264705 & 0.663725777236835 & 0.745048914831862 & 73.3359714026514\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right], \\quad \\left[\\begin{matrix}204.97147134049\\\\174.319793561599\\\\73.3359714026514\\\\0.727736007073887\\\\0.0661557877750238\\\\0.296096138863224\\end{matrix}\\right]\\right )$$"
      ],
      "text/plain": [
       "⎛⎡ 0.954390573728167   -0.175813402219865  0.241305367488368   204.97147134049\n",
       "⎜⎢                                                                            \n",
       "⎜⎢ 0.291150177124445    0.72701976605264   -0.621831033424007  174.31979356159\n",
       "⎜⎢                                                                            \n",
       "⎜⎢-0.0661075422264705  0.663725777236835   0.745048914831862   73.335971402651\n",
       "⎜⎢                                                                            \n",
       "⎜⎣         0                   0                   0                 1.0      \n",
       "⎜                                                                             \n",
       "⎜                                                                             \n",
       "⎜                                                                             \n",
       "⎝                                                                             \n",
       "\n",
       " ⎤, ⎡ 204.97147134049  ⎤⎞\n",
       " ⎥  ⎢                  ⎥⎟\n",
       "9⎥  ⎢ 174.319793561599 ⎥⎟\n",
       " ⎥  ⎢                  ⎥⎟\n",
       "4⎥  ⎢ 73.3359714026514 ⎥⎟\n",
       " ⎥  ⎢                  ⎥⎟\n",
       " ⎦  ⎢0.727736007073887 ⎥⎟\n",
       "    ⎢                  ⎥⎟\n",
       "    ⎢0.0661557877750238⎥⎟\n",
       "    ⎢                  ⎥⎟\n",
       "    ⎣0.296096138863224 ⎦⎠"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "fLeftHandH25(0.1,0.2,0.3,0.1,0.4)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "径核查与 MATLAB fLeftHandH25 函数结果相同"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 符号分析"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def sDH(a, alpha, d, theta):\n",
    "    from sympy import cos,sin\n",
    "    Taf = [[cos(theta),            -sin(theta),            0,              a],\n",
    "    [sin(theta)*cos(alpha), cos(theta)*cos(alpha),  -sin(alpha),    -sin(alpha)*d],\n",
    "    [sin(theta)*sin(alpha), cos(theta)*sin(alpha),  cos(alpha),     cos(alpha)*d],\n",
    "    [0,                     0,                      0,              1]]\n",
    "    return Matrix(Taf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def symbol_dh(n):\n",
    "    from sympy import Symbol\n",
    "    a = Symbol('a_%d' % n)\n",
    "    alpha = Symbol('alpha_%d' % n)\n",
    "    d = Symbol('d_%d' % n)\n",
    "    theta = Symbol('theta_%d' % n)\n",
    "    T = sDH(a,alpha,d,theta)\n",
    "    return a,alpha,d,theta,T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "重点研究中间运动链的正拟运动学, 其中:\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "C++ 源代码, DH 定义, NAOKinematics.cpp\n",
    "\n",
    "``` C++\n",
    "void NAOKinematics::prepareForward(KDeviceLists::ChainsNames ch)\n",
    "{\n",
    "\tif(ch==CHAIN_L_ARM || ch==CHAINS_SIZE)\n",
    "\t{\n",
    "\t\tKMatTransf::makeDHTransformation(T[L_ARM+SHOULDER_PITCH],0.0, -PI_2, 0.0, (double)joints[L_ARM+SHOULDER_PITCH]);\n",
    "\t\tKMatTransf::makeDHTransformation(T[L_ARM+SHOULDER_ROLL], 0.0, PI_2, 0.0, (double)joints[L_ARM+SHOULDER_ROLL] + PI_2);\n",
    "\t\tKMatTransf::makeDHTransformation(T[L_ARM+ELBOW_YAW], +ElbowOffsetY, PI_2, UpperArmLength, (double)joints[L_ARM+ELBOW_YAW] );\n",
    "\t\tKMatTransf::makeDHTransformation(T[L_ARM+ELBOW_ROLL], 0.0, -PI_2, 0.0, (double)joints[L_ARM+ELBOW_ROLL]);\n",
    "\t\t/*Correct one BUT THE FIX does not change anythging*/\n",
    "\t\t//KMatTransf::makeDHTransformation(T[L_ARM+WRIST_YAW], 0.0, PI_2, LowerArmLength, (double)joints[L_ARM+WRIST_YAW]);\n",
    "\t\t/*The fix*/\n",
    "\t\tKMatTransf::makeDHTransformation(T[L_ARM+WRIST_YAW], 0.0, PI_2, 0.0, (double)joints[L_ARM+WRIST_YAW]);\n",
    "\t}\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "UpperArmLength = sympy.Symbol(\"l_1\")\n",
    "ElbowOffsetY = sympy.Symbol(\"l_2\")\n",
    "PI_2 = sympy.pi/2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "sT1 = sDH(0           ,-PI_2,0             ,sympy.Symbol('theta_1'))\n",
    "sT2 = sDH(0           , PI_2,0             ,sympy.Symbol('theta_2')+PI_2)\n",
    "sT3 = sDH(ElbowOffsetY, PI_2,UpperArmLength,sympy.Symbol('theta_3'))\n",
    "sT4 = sDH(0           ,-PI_2,0             ,sympy.Symbol('theta_4'))\n",
    "sT5 = sDH(0           , PI_2,0             ,sympy.Symbol('theta_5'))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "sT15=sT1*sT2*sT3*sT4*sT5\n",
    "sT25=sT2*sT3*sT4*sT5\n",
    "sT35=sT3*sT4*sT5\n",
    "sT45=sT4*sT5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "UpperArmLength = sympy.Symbol(\"l_1\")\n",
    "ElbowOffsetY = sympy.Symbol(\"l_2\")\n",
    "PI_2 = sympy.pi/2\n",
    "\n",
    "sT1 = sDH(0           ,-PI_2,0             ,sympy.Symbol('theta_1'))\n",
    "sT2 = sDH(0           , PI_2,0             ,sympy.Symbol('theta_2')+PI_2)\n",
    "sT3 = sDH(ElbowOffsetY, PI_2,UpperArmLength,sympy.Symbol('theta_3'))\n",
    "sT4 = sDH(0           ,-PI_2,0             ,sympy.Symbol('theta_4'))\n",
    "sT5 = sDH(0           , PI_2,0             ,sympy.Symbol('theta_5'))\n",
    "\n",
    "sT15=sT1*sT2*sT3*sT4*sT5\n",
    "sT25=sT2*sT3*sT4*sT5\n",
    "sT35=sT3*sT4*sT5\n",
    "sT45=sT4*sT5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "def printTf(T,elementList=None):\n",
    "    from IPython.display import display\n",
    "    if elementList is None:\n",
    "        for i in range(3):\n",
    "            for j in range(4):\n",
    "                r = sympy.Symbol(\"r_{%d,%d}\"%(i+1,j+1))\n",
    "                display(sympy.Eq(r,T[i,j]))\n",
    "    else:\n",
    "        for e in elementList:\n",
    "            i,j = e\n",
    "            r = sympy.Symbol(\"r_{%d,%d}\"%(i+1,j+1))\n",
    "            display(sympy.Eq(r,T[i,j]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 用虚拟nao验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from nao import Nao\n",
    "if connect_robot:\n",
    "    nao = Nao(\"127.0.0.1\",9559)\n",
    "    nao.simulationInitPose()\n",
    "else:\n",
    "    nao = Nao() # 仅能使用转换函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.786008536816, \\quad 0.0466809943318, \\quad -1.17838895321, \\quad -0.835045695305, \\quad -0.299782127142, \\quad 0.0\\right ]$$"
      ],
      "text/plain": [
       "[0.786008536816, 0.0466809943318, -1.17838895321, -0.835045695305, -0.29978212\n",
       "7142, 0.0]"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    angles = nao.getLArmAngles()\n",
    "else:\n",
    "    angles = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142,0.0]\n",
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.786008536816, \\quad 0.0466809943318, \\quad -1.17838895321, \\quad -0.835045695305, \\quad -0.299782127142\\right ]$$"
      ],
      "text/plain": [
       "[0.786008536816, 0.0466809943318, -1.17838895321, -0.835045695305, -0.29978212\n",
       "7142]"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[:5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "calTf,calPos = fLeftHandH25(*angles[:5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nparray = np.array(calTf.tolist(),dtype=np.float32)[:3,:]\n",
    "nparray[:,-1]*=0.001 # 单位从mm换算到m"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.967762, 0.00177877, 0.251861, 0.18056\n",
       "           -0.251867, 0.00542309, 0.967747, 0.0773332\n",
       "           0.000355533, -0.999984, 0.00569627, 0.026255])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    Tf = nao.getLArmTf()\n",
    "else:\n",
    "    Tf = almath.Transform([0.967762, 0.00177877, 0.251861, 0.18056,\n",
    "           -0.251867, 0.00542309, 0.967747, 0.0773332,\n",
    "           0.000355533, -0.999984, 0.00569627, 0.026255])\n",
    "Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$4.63828570219e-07$$"
      ],
      "text/plain": [
       "4.63828570219e-07"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tf.distance(nao.nparray2Tf(nparray))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=0.0773332, z=0.026255, wx=-1.5651, wy=-0.000355457, wz=-0.254609)"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    Pos = nao.getLArmPos()\n",
    "else:\n",
    "    Pos = almath.Position6D([0.18056,0.0773332, 0.026255, -1.5651, -0.000355457, -0.254609])\n",
    "Pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# LArm 逆运动学分析"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**已知参数**:\n",
    "\n",
    "* 从最后一个坐标系到末端点,即工具坐标变换 $T_{tool}$, 就是正运动学中的 Tend 矩阵\n",
    "* 从中心到第一个坐标系,即基变换 $T_{base}$, 就是正运动学中的 Base 矩阵\n",
    "* 末端点姿态,即末端矩阵 $T_{end}$, 就是正运动学中的 Tendend 矩阵"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**未知参数**\n",
    "\n",
    "各轴角度,即 $\\theta_1 \\rightarrow \\theta_5$.\n",
    "\n",
    "由正运动学可知:\n",
    "\n",
    "$$\n",
    "T_{end} = T_{base}*{}_5^1T*T_{tool} \n",
    "$$\n",
    "\n",
    "$$\n",
    "{}_5^1T = T_1*T_2*T_3*T_4*T_5\n",
    "$$\n",
    "\n",
    "将运动连矩阵孤立出来有:\n",
    "\n",
    "$$\n",
    "{}_5^1T = (T_{base})^{-1}*T_{end}*(T_{tool})^{-1}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 工具坐标系 T_tool\n",
    "Tool = Matrix.eye(4)\n",
    "Tool[0,3] = LowerArmLength + HandOffsetX\n",
    "Tool[2,3] = -HandOffsetZ\n",
    "R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "Tool = R*Tool"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 基坐标系 T_base\n",
    "Base = Matrix.eye(4)\n",
    "Base[1,3] = shoulderOffsetY\n",
    "Base[2,3] = shoulderOffsetZ"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 目标矩阵 T_end\n",
    "Tend = fLeftHandH25(*angles[:5])[0] # 上一节得到"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 运动连矩阵\n",
    "T15=Base.inv()*Tend*Tool.inv() # 后面证明这是个错误,或者说会导致误差变得很大"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求 $\\theta_1$\n",
    "\n",
    "由运动学的符号分析可知,对于  51T15T  的元素,有:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,4} = l_{1} \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} - l_{2} \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{1} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,4} = l₁⋅cos(θ₁)⋅cos(θ₂) - l₂⋅sin(θ₂)⋅cos(θ₁)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,4} = - l_{1} \\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} + l_{2} \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,4} = -l₁⋅sin(θ₁)⋅cos(θ₂) + l₂⋅sin(θ₁)⋅sin(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT15,[(0,3),(2,3)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "其中\n",
    "\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "即:\n",
    "$$\n",
    "r_{1,4} = \\cos{\\left (\\theta_{1} \\right )}(l_{1} \\cos{\\left (\\theta_{2} \\right )} - l_{2} \\sin{\\left (\\theta_{2} \\right )} )\n",
    "$$\n",
    "\n",
    "$$\n",
    "r_{3,4} = -\\sin{\\left (\\theta_{1} \\right )}(l_{1}  \\cos{\\left (\\theta_{2} \\right )} - l_{2} \\sin{\\left (\\theta_{2} \\right )})\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "所以:\n",
    "\n",
    "$$\n",
    "\\theta_1 = atan(-\\frac{r_{3,4}}{r_{1,4}})\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "因为 $\\theta_1 \\in [-2.0857,2.0857]$ 而 $atan$ 的周期为 $\\pi$ 所以可能存在两个解."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.786008536816$$"
      ],
      "text/plain": [
       "0.786008536816"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.889390931212$$"
      ],
      "text/plain": [
       "0.889390931212"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "theta1 = atan(-T15[2,3]/T15[0,3])\n",
    "theta1"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 问题: 误差太大\n",
    "\n",
    "查看 C++ 版的源代码,并将带入:\n",
    "\n",
    "``` C++\n",
    "\tjoints[L_ARM+SHOULDER_PITCH]=0.78600853;\n",
    "\tjoints[L_ARM+SHOULDER_ROLL]=0.046680994;\n",
    "\tjoints[L_ARM+ELBOW_YAW]=-1.1783889;\n",
    "\tjoints[L_ARM+ELBOW_ROLL]=-0.8350456;\n",
    "\tjoints[L_ARM+WRIST_YAW]=-0.2997821;\n",
    "```\n",
    "\n",
    "得到正运动学解:\n",
    "\n",
    "``` C++\n",
    "0.967762 0.00177858 0.251861 180.56\n",
    "\n",
    "-0.251867 0.0054231 0.967747 77.3332\n",
    "\n",
    "0.000355353 -0.999984 0.00569624 26.2549\n",
    "```\n",
    "Python 得到的正运动学解:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.96776186635391 & 0.00177868767021462 & 0.25186068828098 & 180.559523243616\\\\-0.251866718076071 & 0.00542304637281611 & 0.967746736958398 & 77.3331758678143\\\\0.000355466996881117 & -0.999983713286477 & 0.00569620970505968 & 26.2549592611511\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡  0.96776186635391    0.00177868767021462   0.25186068828098    180.559523243\n",
       "⎢                                                                             \n",
       "⎢ -0.251866718076071   0.00542304637281611   0.967746736958398   77.3331758678\n",
       "⎢                                                                             \n",
       "⎢0.000355466996881117  -0.999983713286477   0.00569620970505968  26.2549592611\n",
       "⎢                                                                             \n",
       "⎣         0                     0                    0                 1.0    \n",
       "\n",
       "616⎤\n",
       "   ⎥\n",
       "143⎥\n",
       "   ⎥\n",
       "511⎥\n",
       "   ⎥\n",
       "   ⎦"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tend"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "两者相同, 按照 C++ 代码看求解 $\\theta_1$ 的不同:\n",
    "\n",
    "``` C++\n",
    "NAOKinematics::AngleContainer NAOKinematics::inverseLeftHand(kmatTable targetPoint)\n",
    "{\n",
    "  ... ...\n",
    "\tTinit = targetPoint; // T_end\n",
    "\tTinit.fast_invert();// (T_end)^-1\n",
    "\tTemp = T[FR_END_T+CHAIN_L_ARM];// T_tool\n",
    "\tTemp*=Tinit; // T_tool*(T_end)^-1\n",
    "\tTemp*=T[FR_BASE_T+CHAIN_L_ARM];//T_tool*(T_end)^-1*(T_base)\n",
    "\ttry\n",
    "\t{\n",
    "\t\tTemp.fast_invert();// (T_tool*(T_end)^-1*(T_base))^-1\n",
    "\t}\n",
    "\tcatch(KMath::KMat::SingularMatrixInvertionException d)\n",
    "\t{\n",
    "\t\treturn returnResult;\n",
    "\t}\n",
    "\ttheta1 = atan2(-Temp(2,3),Temp(0,3));\n",
    "    ... ...\n",
    "}\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "T_end:\n",
    "\n",
    "0.967762 0.00177858 0.251861 180.56\n",
    "\n",
    "-0.251867 0.0054231 0.967747 77.3332\n",
    "\n",
    "0.000355353 -0.999984 0.00569624 26.2549\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.96776186635391 & 0.00177868767021462 & 0.25186068828098 & 180.559523243616\\\\-0.251866718076071 & 0.00542304637281611 & 0.967746736958398 & 77.3331758678143\\\\0.000355466996881117 & -0.999983713286477 & 0.00569620970505968 & 26.2549592611511\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡  0.96776186635391    0.00177868767021462   0.25186068828098    180.559523243\n",
       "⎢                                                                             \n",
       "⎢ -0.251866718076071   0.00542304637281611   0.967746736958398   77.3331758678\n",
       "⎢                                                                             \n",
       "⎢0.000355466996881117  -0.999983713286477   0.00569620970505968  26.2549592611\n",
       "⎢                                                                             \n",
       "⎣         0                     0                    0                 1.0    \n",
       "\n",
       "616⎤\n",
       "   ⎥\n",
       "143⎥\n",
       "   ⎥\n",
       "511⎥\n",
       "   ⎥\n",
       "   ⎦"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tend"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(T_end)^-1:\n",
    "\n",
    "0.967762 -0.251867 0.000355353 -155.27\n",
    "\n",
    "0.00177858 0.0054231 -0.999984 25.514\n",
    "\n",
    "0.251861 0.967747 0.00569624 -120.464"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.96776186635391 & -0.251866718076071 & 0.000355466996881195 & -155.27030076951\\\\0.00177868767021039 & 0.00542304637281177 & -0.999983713286477 & 25.5139912575323\\\\0.25186068828098 & 0.967746736958398 & 0.00569620970505965 & -120.464328158279\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡ 0.96776186635391    -0.251866718076071   0.000355466996881195  -155.27030076\n",
       "⎢                                                                             \n",
       "⎢0.00177868767021039  0.00542304637281177   -0.999983713286477   25.5139912575\n",
       "⎢                                                                             \n",
       "⎢ 0.25186068828098     0.967746736958398   0.00569620970505965   -120.46432815\n",
       "⎢                                                                             \n",
       "⎣         0                    0                    0                   1.0   \n",
       "\n",
       "951 ⎤\n",
       "    ⎥\n",
       "323 ⎥\n",
       "    ⎥\n",
       "8279⎥\n",
       "    ⎥\n",
       "    ⎦"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tend.inv()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "T_tool:\n",
    "    \n",
    "6.12323e-17 1 0 6.96212e-15\n",
    "\n",
    "-6.12323e-17 3.7494e-33 1 -12.31\n",
    "\n",
    "1 -6.12323e-17 6.12323e-17 113.7\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}6.12323399573677 \\cdot 10^{-17} & 1.0 & 0 & 6.9621170531527 \\cdot 10^{-15}\\\\-6.12323399573677 \\cdot 10^{-17} & 3.74939945665464 \\cdot 10^{-33} & 1.0 & -12.31\\\\1.0 & -6.12323399573677 \\cdot 10^{-17} & 6.12323399573677 \\cdot 10^{-17} & 113.7\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡6.12323399573677e-17            1.0                    0            6.9621170\n",
       "⎢                                                                             \n",
       "⎢-6.12323399573677e-17  3.74939945665464e-33           1.0                 -12\n",
       "⎢                                                                             \n",
       "⎢         1.0           -6.12323399573677e-17  6.12323399573677e-17         11\n",
       "⎢                                                                             \n",
       "⎣          0                      0                     0                    1\n",
       "\n",
       "531527e-15⎤\n",
       "          ⎥\n",
       ".31       ⎥\n",
       "          ⎥\n",
       "3.7       ⎥\n",
       "          ⎥\n",
       ".0        ⎦"
      ]
     },
     "execution_count": 33,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tool"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "T_tool*(T_end)^-1:\n",
    "    \n",
    "0.00177858 0.0054231 -0.999984 25.514\n",
    "\n",
    "0.251861 0.967747 0.00569624 -132.774\n",
    "\n",
    "0.967762 -0.251867 0.000355353 -41.5703"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.00177868767021045 & 0.00542304637281175 & -0.999983713286477 & 25.5139912575323\\\\0.25186068828098 & 0.967746736958398 & 0.00569620970505965 & -132.774328158279\\\\0.96776186635391 & -0.251866718076071 & 0.000355466996881257 & -41.5703007695098\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.00177868767021045  0.00542304637281175   -0.999983713286477   25.5139912575\n",
       "⎢                                                                             \n",
       "⎢ 0.25186068828098     0.967746736958398   0.00569620970505965   -132.77432815\n",
       "⎢                                                                             \n",
       "⎢ 0.96776186635391    -0.251866718076071   0.000355466996881257  -41.570300769\n",
       "⎢                                                                             \n",
       "⎣         0                    0                    0                   1.0   \n",
       "\n",
       "323 ⎤\n",
       "    ⎥\n",
       "8279⎥\n",
       "    ⎥\n",
       "5098⎥\n",
       "    ⎥\n",
       "    ⎦"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tool*Tend.inv()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "T_tool*(T_end)^-1*(T_base):\n",
    "    \n",
    "0.00177858 0.0054231 -0.999984 -73.9529\n",
    "\n",
    "0.251861 0.967747 0.00569624 -37.3655\n",
    "\n",
    "0.967762 -0.251867 0.000355353 -66.2177"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.00177868767021045 & 0.00542304637281175 & -0.999983713286477 & -73.9529215265798\\\\0.25186068828098 & 0.967746736958398 & 0.00569620970505965 & -37.3655269658502\\\\0.96776186635391 & -0.251866718076071 & 0.000355466996881257 & -66.2176924412766\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.00177868767021045  0.00542304637281175   -0.999983713286477   -73.952921526\n",
       "⎢                                                                             \n",
       "⎢ 0.25186068828098     0.967746736958398   0.00569620970505965   -37.365526965\n",
       "⎢                                                                             \n",
       "⎢ 0.96776186635391    -0.251866718076071   0.000355466996881257  -66.217692441\n",
       "⎢                                                                             \n",
       "⎣         0                    0                    0                   1.0   \n",
       "\n",
       "5798⎤\n",
       "    ⎥\n",
       "8502⎥\n",
       "    ⎥\n",
       "2766⎥\n",
       "    ⎥\n",
       "    ⎦"
      ]
     },
     "execution_count": 35,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tool*Tend.inv()*Base"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(T_tool*(T_end)^-1*(T_base))^-1:\n",
    "\n",
    "0.00177858 0.251861 0.967762 73.6254\n",
    "\n",
    "0.0054231 0.967747 -0.251867 19.8834\n",
    "\n",
    "-0.999984 0.00569624 0.000355353 -73.7153\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.00177868766968459 & 0.25186068828098 & 0.96776186635391 & 73.6254041118955\\\\0.00542304637303914 & 0.967746736958398 & -0.251866718076071 & 19.8833840450243\\\\-0.999983713286477 & 0.00569620970505438 & 0.000355466996878258 & -73.715336994923\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.00177868766968459   0.25186068828098      0.96776186635391    73.6254041118\n",
       "⎢                                                                             \n",
       "⎢0.00542304637303914   0.967746736958398    -0.251866718076071   19.8833840450\n",
       "⎢                                                                             \n",
       "⎢-0.999983713286477   0.00569620970505438  0.000355466996878258  -73.715336994\n",
       "⎢                                                                             \n",
       "⎣         0                    0                    0                  1.0    \n",
       "\n",
       "955⎤\n",
       "   ⎥\n",
       "243⎥\n",
       "   ⎥\n",
       "923⎥\n",
       "   ⎥\n",
       "   ⎦"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "(Tool*Tend.inv()*Base).inv() # C++ 中的算法,可以提高精度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left[\\begin{matrix}0.00177868767021462 & 0.25186068828098 & 0.96776186635391 & 59.7864094230544\\\\0.00542304637281611 & 0.967746736958398 & -0.251866718076071 & 23.4850781135093\\\\-0.999983713286477 & 0.00569620970505968 & 0.000355466996881178 & -73.7204201729804\\\\0 & 0 & 0 & 1.0\\end{matrix}\\right]$$"
      ],
      "text/plain": [
       "⎡0.00177868767021462   0.25186068828098      0.96776186635391    59.7864094230\n",
       "⎢                                                                             \n",
       "⎢0.00542304637281611   0.967746736958398    -0.251866718076071   23.4850781135\n",
       "⎢                                                                             \n",
       "⎢-0.999983713286477   0.00569620970505968  0.000355466996881178  -73.720420172\n",
       "⎢                                                                             \n",
       "⎣         0                    0                    0                   1.0   \n",
       "\n",
       "544 ⎤\n",
       "    ⎥\n",
       "093 ⎥\n",
       "    ⎥\n",
       "9804⎥\n",
       "    ⎥\n",
       "    ⎦"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 与 (Tool*Tend.inv()*Base).inv() 在数学上等价,\n",
    "# 但误差很大,特别是在平移的问题上\n",
    "Base.inv()*Tend*Tool.inv()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.786008536816$$"
      ],
      "text/plain": [
       "0.786008536816"
      ]
     },
     "execution_count": 38,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T15 = (Tool*Tend.inv()*Base).inv()\n",
    "theta1 = atan(-T15[2,3]/T15[0,3])\n",
    "theta1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.786008536816$$"
      ],
      "text/plain": [
       "0.786008536816"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 求 $\\theta_2$\n",
    "\n",
    "现在可以将 $T1$ 从变换链中剔除,得到 ${}_5^2 T$,由运动学的符号分析可知,对于  ${}_5^2 T$ 的元素,有:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,4} = l_{1} \\cos{\\left (\\theta_{2} \\right )} - l_{2} \\sin{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,4} = l₁⋅cos(θ₂) - l₂⋅sin(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,4} = l_{1} \\sin{\\left (\\theta_{2} \\right )} + l_{2} \\cos{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,4} = l₁⋅sin(θ₂) + l₂⋅cos(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT25,[(0,3),(2,3)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "其中:\n",
    "\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "这是关于 $\\theta_2$ 的方程组, 只有一个变量,却由两个方程.\n",
    "\n",
    "显然,机械臂的是5个自由度而不是6个自由度的结果就是,只能达到六维空间的一个子空间,猜测是个5维空间\n",
    "\n",
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_2 = atan2(l_1*r_{3,4}-l_2*r_{1,4}, l_1*r_{1,4}+l_2*r_{3,4})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T1 = DH(0,-pi/2,0,theta1)\n",
    "T25 = T1.inv()*T15"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "l1 = upperArmLength\n",
    "l2 = elbowOffsetY\n",
    "r14 = T25[0,3]\n",
    "r34 = T25[2,3]\n",
    "theta2 = atan2(l1*r34-l2*r14,l1*r14+l2*r34)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.0466809943318$$"
      ],
      "text/plain": [
       "0.0466809943318"
      ]
     },
     "execution_count": 43,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### 求 $\\theta_3$\n",
    "\n",
    "现在可以将 $T2$ 从变换链中剔除,得到 ${}_5^3 T$,由运动学的符号分析可知,对于  ${}_5^3 T$ 的元素,有::"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{3,3} = \\sin{\\left (\\theta_{3} \\right )} \\sin{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,3} = sin(θ₃)⋅sin(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{1,3} = \\sin{\\left (\\theta_{4} \\right )} \\cos{\\left (\\theta_{3} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,3} = sin(θ₄)⋅cos(θ₃)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT35,[(2,2),(0,2)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_3 = atan2(-r_{3,3},-r_{1,3})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-1.17838895321$$"
      ],
      "text/plain": [
       "-1.17838895321"
      ]
     },
     "execution_count": 45,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T2 = DH(0,pi/2,0,theta2+pi/2)\n",
    "T35 = T2.inv()*T25\n",
    "theta3 = atan2(-T35[2,2],-T35[0,2])\n",
    "theta3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-1.17838895321$$"
      ],
      "text/plain": [
       "-1.17838895321"
      ]
     },
     "execution_count": 46,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[2]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### 求 $\\theta_4$\n",
    "\n",
    "现在可以将 $T3$ 从变换链中剔除,得到 ${}_5^4 T$,由运动学的符号分析可知,对于  ${}_5^4 T$ 的元素,有::"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,3} = \\sin{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,3} = sin(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,3} = \\cos{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,3} = cos(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT45,[(0,2),(2,2)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_4 = atan2(r_{1,3},r_{3,3})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.835045695305$$"
      ],
      "text/plain": [
       "-0.835045695305"
      ]
     },
     "execution_count": 48,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T3 = DH(elbowOffsetY,pi/2,upperArmLength,theta3)\n",
    "T45 = T3.inv()*T35\n",
    "theta4 = atan2(T45[0,2],T45[2,2])\n",
    "theta4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.835045695305$$"
      ],
      "text/plain": [
       "-0.835045695305"
      ]
     },
     "execution_count": 49,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[3]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "### 求 $\\theta_5$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{2,1} = \\sin{\\left (\\theta_{5} \\right )}$$"
      ],
      "text/plain": [
       "r_{2,1} = sin(θ₅)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{2,2} = \\cos{\\left (\\theta_{5} \\right )}$$"
      ],
      "text/plain": [
       "r_{2,2} = cos(θ₅)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT45,[(1,0),(1,1)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_5 = atan2(r_{2,1}, r_{2,2})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.299782127142$$"
      ],
      "text/plain": [
       "-0.299782127142"
      ]
     },
     "execution_count": 51,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "theta5 = atan2(T45[1,0],T45[1,1])\n",
    "theta5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.299782127142$$"
      ],
      "text/plain": [
       "-0.299782127142"
      ]
     },
     "execution_count": 52,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[4]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# RArm 正运动学分析\n",
    "\n",
    "基本原理与LArm相同"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def fRightHandH25(theta1,theta2,theta3,theta4,theta5):\n",
    "    global shoulderOffsetY\n",
    "    global LowerArmLength\n",
    "    global elbowOffsetY\n",
    "    global shoulderOffsetZ\n",
    "    global HandOffsetX\n",
    "    global upperArmLength\n",
    "    global HandOffsetZ\n",
    "    \n",
    "    base = Matrix.eye(4)\n",
    "    base[1,3] = -shoulderOffsetY # 与LArm不同\n",
    "    base[2,3] = shoulderOffsetZ\n",
    "    \n",
    "    T1 = DH(0,-pi/2,0,theta1)\n",
    "    T2 = DH(0,pi/2,0,theta2+pi/2)\n",
    "    T3 = DH(-elbowOffsetY,pi/2,upperArmLength,theta3) # 与LArm不同\n",
    "    T4 = DH(0,-pi/2,0,theta4)\n",
    "    T5 = DH(0,pi/2,0,theta5)\n",
    "    Tend1 = Matrix.eye(4)\n",
    "    Tend1[0,3] = LowerArmLength + HandOffsetX\n",
    "    Tend1[2,3] = -HandOffsetZ\n",
    "    R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "    Tend = R*Tend1\n",
    "    Tendend = base*T1*T2*T3*T4*T5*Tend\n",
    "    \n",
    "    rotZ = atan2(Tendend[1,0],Tendend[0,0])\n",
    "    rotY = atan2(-Tendend[2,0], sqrt(float(Tendend[2,1]**2 + Tendend[2,2]**2)))\n",
    "    rotX = atan2(Tendend[2,1],Tendend[2,2])\n",
    "    left = Matrix.zeros(6,1)\n",
    "    left[0:3,0] = Tendend[0:3,3]\n",
    "    left[3:6,0] = Matrix([rotX,rotY,rotZ])\n",
    "    return Tendend,left"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 符号分析\n",
    "\n",
    "重点研究中间运动链的正拟运动学, 其中:\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "C++ 源代码, DH 定义, NAOKinematics.cpp\n",
    "\n",
    "``` C++\n",
    "void NAOKinematics::prepareForward(KDeviceLists::ChainsNames ch)\n",
    "{\n",
    "    ... ...\n",
    "\tif(ch==CHAIN_R_ARM || ch==CHAINS_SIZE)\n",
    "\t{\n",
    "\t\tKMatTransf::makeDHTransformation(T[R_ARM+SHOULDER_PITCH], 0.0, -PI_2, 0.0, (double)joints[R_ARM+SHOULDER_PITCH]);\n",
    "\t\tKMatTransf::makeDHTransformation(T[R_ARM+SHOULDER_ROLL], 0.0, PI_2, 0.0, (double)joints[R_ARM+SHOULDER_ROLL] + PI_2);\n",
    "\t\tKMatTransf::makeDHTransformation(T[R_ARM+ELBOW_YAW], -ElbowOffsetY, PI_2, UpperArmLength, (double) joints[R_ARM+ELBOW_YAW]);\n",
    "\t\tKMatTransf::makeDHTransformation(T[R_ARM+ELBOW_ROLL], 0.0, -PI_2, 0.0, (double)joints[R_ARM+ELBOW_ROLL]);\n",
    "\t\t/*Correct one BUT THE FIX does not change anythging*/\n",
    "\t\t//KMatTransf::makeDHTransformation(T[R_ARM+WRIST_YAW], 0.0, PI_2, LowerArmLength, (double)joints[R_ARM+WRIST_YAW]);\n",
    "\t\t/*The fix*/\n",
    "\t\tKMatTransf::makeDHTransformation(T[R_ARM+WRIST_YAW], 0.0, PI_2, 0.0, (double)joints[R_ARM+WRIST_YAW]);\n",
    "\t}\n",
    "    ... ...\n",
    "}\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "UpperArmLength = sympy.Symbol(\"l_1\")\n",
    "ElbowOffsetY = sympy.Symbol(\"l_2\")\n",
    "PI_2 = sympy.pi/2\n",
    "\n",
    "sT1 = sDH(0            ,-PI_2,0             ,sympy.Symbol('theta_1'))\n",
    "sT2 = sDH(0            , PI_2,0             ,sympy.Symbol('theta_2')+PI_2)\n",
    "sT3 = sDH(-ElbowOffsetY, PI_2,UpperArmLength,sympy.Symbol('theta_3')) # 与 LArm 不同\n",
    "sT4 = sDH(0            ,-PI_2,0             ,sympy.Symbol('theta_4'))\n",
    "sT5 = sDH(0            , PI_2,0             ,sympy.Symbol('theta_5'))\n",
    "\n",
    "sT15=sT1*sT2*sT3*sT4*sT5\n",
    "sT25=sT2*sT3*sT4*sT5\n",
    "sT35=sT3*sT4*sT5\n",
    "sT45=sT4*sT5"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 用虚拟nao验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.78600859642, \\quad -0.0466810427606, \\quad 1.17838907242, \\quad 0.835045814514, \\quad 0.299782037735, \\quad 0.0\\right ]$$"
      ],
      "text/plain": [
       "[0.78600859642, -0.0466810427606, 1.17838907242, 0.835045814514, 0.29978203773\n",
       "5, 0.0]"
      ]
     },
     "execution_count": 55,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    angles = nao.getRArmAngles()\n",
    "else:\n",
    "    angles = [0.78600859642,-0.0466810427606,1.17838907242,0.835045814514,0.299782037735,0.0]\n",
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left [ 0.78600859642, \\quad -0.0466810427606, \\quad 1.17838907242, \\quad 0.835045814514, \\quad 0.299782037735\\right ]$$"
      ],
      "text/plain": [
       "[0.78600859642, -0.0466810427606, 1.17838907242, 0.835045814514, 0.29978203773\n",
       "5]"
      ]
     },
     "execution_count": 56,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[:5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "calTf,calPos = fRightHandH25(*angles[:5])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nparray = np.array(calTf.tolist(),dtype=np.float32)[:3,:]\n",
    "nparray[:,-1]*=0.001 # 单位从mm换算到m"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Transform([0.967762, -0.0017788, 0.251861, 0.18056\n",
       "           0.251867, 0.00542308, -0.967747, -0.0773332\n",
       "           0.00035557, 0.999984, 0.00569628, 0.026255])"
      ]
     },
     "execution_count": 59,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    Tf = nao.getRArmTf()\n",
    "else:\n",
    "    Tf = almath.Transform([0.967762, -0.0017788, 0.251861, 0.18056,\n",
    "           0.251867, 0.00542308, -0.967747, -0.0773332,\n",
    "           0.00035557, 0.999984, 0.00569628, 0.026255])\n",
    "Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$4.63289694608e-07$$"
      ],
      "text/plain": [
       "4.63289694608e-07"
      ]
     },
     "execution_count": 60,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Tf.distance(nao.nparray2Tf(nparray))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Position6D(x=0.18056, y=-0.0773332, z=0.026255, wx=1.5651, wy=-0.000355457, wz=0.254609)"
      ]
     },
     "execution_count": 61,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "if connect_robot:\n",
    "    Pos = nao.getRArmPos()\n",
    "else:\n",
    "    Pos = almath.Position6D([0.18056,-0.0773332, 0.026255, 1.5651, -0.000355457, 0.254609])\n",
    "Pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# RArm 逆运动学分析\n",
    "\n",
    "基本原理同 LArm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 62,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 工具坐标系 T_tool\n",
    "Tool = Matrix.eye(4)\n",
    "Tool[0,3] = LowerArmLength + HandOffsetX\n",
    "Tool[2,3] = -HandOffsetZ\n",
    "R = RotXYZMatrix(-pi/2,0,-pi/2)\n",
    "Tool = R*Tool"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 63,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 基坐标系 T_base\n",
    "Base = Matrix.eye(4)\n",
    "Base[1,3] = -shoulderOffsetY # 与 LArm 不同\n",
    "Base[2,3] = shoulderOffsetZ"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 64,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 目标矩阵 T_end\n",
    "Tend = fRightHandH25(*angles[:5])[0] # 上一节得到"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 65,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# 运动连矩阵\n",
    "T15 = (Tool*Tend.inv()*Base).inv() # 不用 T15=Base.inv()*Tend*Tool.inv(), 原因同LArm"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 求 $\\theta_1$\n",
    "\n",
    "由运动学的符号分析可知,对于  51T15T  的元素,有:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,4} = l_{1} \\cos{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} + l_{2} \\sin{\\left (\\theta_{2} \\right )} \\cos{\\left (\\theta_{1} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,4} = l₁⋅cos(θ₁)⋅cos(θ₂) + l₂⋅sin(θ₂)⋅cos(θ₁)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,4} = - l_{1} \\sin{\\left (\\theta_{1} \\right )} \\cos{\\left (\\theta_{2} \\right )} - l_{2} \\sin{\\left (\\theta_{1} \\right )} \\sin{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,4} = -l₁⋅sin(θ₁)⋅cos(θ₂) - l₂⋅sin(θ₁)⋅sin(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT15,[(0,3),(2,3)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "其中\n",
    "\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "即:\n",
    "$$\n",
    "r_{1,4} = \\cos{\\left (\\theta_{1} \\right )}(l_{1} \\cos{\\left (\\theta_{2} \\right )} + l_{2} \\sin{\\left (\\theta_{2} \\right )} )\n",
    "$$\n",
    "\n",
    "$$\n",
    "r_{3,4} = -\\sin{\\left (\\theta_{1} \\right )}(l_{1}  \\cos{\\left (\\theta_{2} \\right )} + l_{2} \\sin{\\left (\\theta_{2} \\right )})\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "所以:\n",
    "\n",
    "$$\n",
    "\\theta_1 = atan(-\\frac{r_{3,4}}{r_{1,4}})\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "因为 $\\theta_1 \\in [-2.0857,2.0857]$ 而 $atan$ 的周期为 $\\pi$ 所以可能存在两个解."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 67,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.78600859642$$"
      ],
      "text/plain": [
       "0.78600859642"
      ]
     },
     "execution_count": 67,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T15 = (Tool*Tend.inv()*Base).inv()\n",
    "theta1 = atan(-T15[2,3]/T15[0,3])\n",
    "theta1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 68,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.78600859642$$"
      ],
      "text/plain": [
       "0.78600859642"
      ]
     },
     "execution_count": 68,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[0]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 求 $\\theta_2$\n",
    "\n",
    "现在可以将 $T1$ 从变换链中剔除,得到 ${}_5^2 T$,由运动学的符号分析可知,对于  ${}_5^2 T$ 的元素,有:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 69,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,4} = l_{1} \\cos{\\left (\\theta_{2} \\right )} + l_{2} \\sin{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,4} = l₁⋅cos(θ₂) + l₂⋅sin(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,4} = l_{1} \\sin{\\left (\\theta_{2} \\right )} - l_{2} \\cos{\\left (\\theta_{2} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,4} = l₁⋅sin(θ₂) - l₂⋅cos(θ₂)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT25,[(0,3),(2,3)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "其中:\n",
    "\n",
    "$$l_1 = UpperArmLength$$ \n",
    "$$l_2 = ElbowOffsetY$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_2 = atan2(l_2*r_{1,4}-l_1*(-r_{3,4}), l_2*(-r_{3,4})+l_1*r_{1,4})\n",
    "$$\n",
    "\n",
    "即:\n",
    "\n",
    "$$\n",
    "\\theta_2 = atan2(l_1*r_{3,4}+l_2*r_{1,4}, l_1*r_{1,4}-l_2*r_{3,4})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 70,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T1 = DH(0,-pi/2,0,theta1)\n",
    "T25 = T1.inv()*T15"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 71,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.0466810427607$$"
      ],
      "text/plain": [
       "-0.0466810427607"
      ]
     },
     "execution_count": 71,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "l1 = upperArmLength\n",
    "l2 = elbowOffsetY\n",
    "r14 = T25[0,3]\n",
    "r34 = T25[2,3]\n",
    "theta2 = atan2(l1*r34+l2*r14,l1*r14-l2*r34)\n",
    "theta2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 72,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$-0.0466810427606$$"
      ],
      "text/plain": [
       "-0.0466810427606"
      ]
     },
     "execution_count": 72,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[1]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 求 $\\theta_3$\n",
    "\n",
    "现在可以将 $T2$ 从变换链中剔除,得到 ${}_5^3 T$,由运动学的符号分析可知,对于  ${}_5^3 T$ 的元素,有::"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{3,3} = \\sin{\\left (\\theta_{3} \\right )} \\sin{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,3} = sin(θ₃)⋅sin(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{1,3} = \\sin{\\left (\\theta_{4} \\right )} \\cos{\\left (\\theta_{3} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,3} = sin(θ₄)⋅cos(θ₃)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT35,[(2,2),(0,2)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_3 = atan2(r_{3,3},r_{1,3})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$1.17838907242$$"
      ],
      "text/plain": [
       "1.17838907242"
      ]
     },
     "execution_count": 74,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T2 = DH(0,pi/2,0,-0.0466810427606+pi/2)\n",
    "T35 = T2.inv()*T25\n",
    "theta3 = atan2(T35[2,2],T35[0,2])\n",
    "theta3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$1.17838907242$$"
      ],
      "text/plain": [
       "1.17838907242"
      ]
     },
     "execution_count": 75,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[2]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 求 $\\theta_4$\n",
    "\n",
    "现在可以将 $T3$ 从变换链中剔除,得到 ${}_5^4 T$,由运动学的符号分析可知,对于  ${}_5^4 T$ 的元素,有::"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{1,3} = \\sin{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{1,3} = sin(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{3,3} = \\cos{\\left (\\theta_{4} \\right )}$$"
      ],
      "text/plain": [
       "r_{3,3} = cos(θ₄)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT45,[(0,2),(2,2)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_4 = atan2(r_{1,3},r_{3,3})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.835045814514$$"
      ],
      "text/plain": [
       "0.835045814514"
      ]
     },
     "execution_count": 77,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T3 = DH(-elbowOffsetY,pi/2,upperArmLength,theta3) # 与 LArm 不同\n",
    "T45 = T3.inv()*T35\n",
    "theta4 = atan2(T45[0,2],T45[2,2])\n",
    "theta4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.835045814514$$"
      ],
      "text/plain": [
       "0.835045814514"
      ]
     },
     "execution_count": 78,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[3]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## 求 $\\theta_5$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$r_{2,1} = \\sin{\\left (\\theta_{5} \\right )}$$"
      ],
      "text/plain": [
       "r_{2,1} = sin(θ₅)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$$r_{2,2} = \\cos{\\left (\\theta_{5} \\right )}$$"
      ],
      "text/plain": [
       "r_{2,2} = cos(θ₅)"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "printTf(sT45,[(1,0),(1,1)])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "可得:\n",
    "\n",
    "$$\n",
    "\\theta_5 = atan2(r_{2,1}, r_{2,2})\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.299782037735$$"
      ],
      "text/plain": [
       "0.299782037735"
      ]
     },
     "execution_count": 80,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "theta5 = atan2(T45[1,0],T45[1,1])\n",
    "theta5"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 81,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$0.299782037735$$"
      ],
      "text/plain": [
       "0.299782037735"
      ]
     },
     "execution_count": 81,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "angles[4]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
